﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace ERF.Physics
{
    /// <summary>
    /// Represents gravity logic.
    /// </summary>
    public class Gravitylogic : PhysicsLogic
    {
        private Vector3 gravityVector;

        /// <summary>
        /// Creates a new instance of GravityLogic with the given acceleration.
        /// </summary>
        /// <param name="gravityVector">The acceleration of the gravity in m/s^2.</param>
        public Gravitylogic(Vector3 gravityVector)
        {
            this.gravityVector = gravityVector;
        }

        public override void ApplyOnBody(float timeSinceLastUpdate, Body body)
        {
            if (!body.isStatic)
            {
                // Calculate the force of the gravity given the bodys mass.
                Vector3 gravityForce = body.mass * gravityVector;

                // If the body is on the ground, we need to calculate the net force given the surface it stands on.
                if (body.isOnGround)
                {

                    Vector3 N = base.parentEngine.scene.GetNormalAtXZ(body.position.X, body.position.Z);

                    // Vector3.Zero is returned when attempting to retrieve a normal outside of the scenes bounds.
                    if (N != Vector3.Zero)
                    {
                        // Angle between the gravitational force and the surface normal.
                        float theta = (float)Math.Acos(Vector3.Dot(gravityForce, N) / (gravityForce.Length() * N.Length()));

                        // FG_N is the force along the surface normal.
                        Vector3 FG_N = -gravityForce.Length() * (float)Math.Cos(Math.PI - theta) * N;

                        // Since the gravitational force is equal to FG_N + FG_S (where FG_S is the force along the surface slope):
                        Vector3 FG_S = gravityForce - FG_N;

                        // Calculate static friction.
                        float staticFrictionCoefficient = base.parentEngine.Scene.StaticFrictionCoefficient;
                        float staticFrictionLength = FG_N.Length() * staticFrictionCoefficient;
                        float slidingForceLength = FG_S.Length();
                        if (slidingForceLength <= staticFrictionLength)
                            staticFrictionLength = slidingForceLength;

                        Vector3 staticFriction;
                        if(slidingForceLength != 0)
                            staticFriction = staticFrictionLength * -Vector3.Normalize(FG_S);
                        else
                            staticFriction = Vector3.Zero;

                        body.force += FG_S + staticFriction;
                    }
                }
                else
                {
                    // If the body is NOT on the ground, just add the gravitational force to its forces.
                    body.force += gravityForce;
                }
            }
        }
    }
}
